#include "mlVector.h"
#include "mlExtras.h"
#include <math.h>
#include <stdio.h>
#include <string.h>

const float
mlVectorX[3] = { 1.0f, 0.0f, 0.0f }, mlVectorY[3] = { 0.0f, 1.0f, 0.0f }, mlVectorZ[3] = { 0.0f, 0.0f, 1.0f };

const float
mlVectorNegX[3] = { -1.0f, 0.0f, 0.0f }, mlVectorNegY[3] = { 0.0f, -1.0f, 0.0f }, mlVectorNegZ[3] = { 0.0f, 0.0f, -1.0f };

void
mlVectorPrint(const float *a)
{
	printf("(%f, %f, %f)\n", a[0], a[1], a[2]);
}

void
mlVectorCopy(const float *a, float *result)
{
	memcpy(result, a, 3 * sizeof(float));
}

void
mlVectorLoad(float *a, float x, float y, float z)
{
	a[0] = x; a[1] = y; a[2] = z;
}

void
mlVectorLoadX(float *a)
{
	mlVectorLoad(a, 1.0f, 0.0f, 0.0f);
}

void
mlVectorLoadY(float *a)
{
	mlVectorLoad(a, 0.0f, 1.0f, 0.0f);
}

void
mlVectorLoadZ(float *a)
{
	mlVectorLoad(a, 0.0f, 0.0f, 1.0f);
}

void
mlVectorLoadNegX(float *a)
{
	mlVectorLoad(a, -1.0f, 0.0f, 0.0f);
}

void
mlVectorLoadNegY(float *a)
{
	mlVectorLoad(a, 0.0f, -1.0f, 0.0f);
}

void
mlVectorLoadNegZ(float *a)
{
	mlVectorLoad(a, 0.0f, 0.0f, -1.0f);
}

void
mlVectorAddTo(const float *a, const float *b, float *result)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		result[i] = a[i] + b[i];
}

void
mlVectorAdd(float *a, const float *b)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		a[i] += b[i];
}

void
mlVectorSubTo(const float *a, const float *b, float *result)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		result[i] = a[i] - b[i];
}

void
mlVectorSub(float *a, const float *b)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		a[i] -= b[i];
}

void
mlVectorScaleTo(const float *a, float b, float *result)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		result[i] = a[i] * b;
}

void
mlVectorScale(float *a, float b)
{
	unsigned int i;

	for(i = 0; i < 3; i++)
		a[i] *= b;
}

void
mlVectorNormTo(const float *a, float *result)
{
	*result = sqrtf(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
}

float
mlVectorNorm(const float *a)
{
	return sqrtf(a[0] * a[0] + a[1] * a[1] + a[2] * a[2]);
}

void
mlVectorDotProductTo(const float *a, const float *b, float *result)
{
	*result = a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

float
mlVectorDotProduct(const float *a, const float *b)
{
	return a[0] * b[0] + a[1] * b[1] + a[2] * b[2];
}

void
mlVectorCrossProductTo(const float *a, const float *b, float *result)
{
	result[0] = a[1] * b[2] - a[2] * b[1];
	result[1] = a[2] * b[0] - a[0] * b[2];
	result[2] = a[0] * b[1] - a[1] * b[0];
}

void
mlVectorCrossProduct(float *a, const float *b)
{
	float result[3];

	mlVectorCrossProductTo(a, b, result);
	mlVectorCopy(result, a);
}

void
mlVectorNormalTo(const float *a, float *result)
{
	mlVectorScaleTo(a, 1.0f / mlVectorNorm(a), result);
}

void
mlVectorNormal(float *a)
{
	mlVectorScale(a, 1.0f / mlVectorNorm(a));
}

void
mlVectorMultMatrixTo(const float *a, const float *b, float *result)
{
	result[0] = b[0] * a[0] + b[4] * a[1] + b[8] * a[2] + b[12];
	result[1] = b[1] * a[0] + b[5] * a[1] + b[9] * a[2] + b[13];
	result[2] = b[2] * a[0] + b[6] * a[1] + b[10] * a[2] + b[14];
}

void
mlVectorMultMatrix(float *a, const float *b)
{
	float result[3];

	mlVectorMultMatrixTo(a, b, result);
	mlVectorCopy(result, a);
}

void
mlVectorRotateTo(const float *a, const float *axis, float angle, float *result)
{
	const float c = cosf(mlDegToRad(angle)), s = sinf(mlDegToRad(angle));
	const float matrix[16] =
	{
		axis[0] * axis[0] * (1.0f - c) + c, axis[1] * axis[0] * (1.0f - c) + axis[2] * s, axis[0] * axis[2] * (1.0f - c) - axis[1] * s, 0.0f,
		axis[0] * axis[1] * (1.0f - c) - axis[2] * s, axis[1] * axis[1] * (1.0f - c) + c, axis[1] * axis[2] * (1.0f - c) + axis[0] * s, 0.0f,
		axis[0] * axis[2] * (1.0f - c) + axis[1] * s, axis[1] * axis[2] * (1.0f - c) - axis[0] * s, axis[2] * axis[2] * (1.0f - c) + c, 0.0f,
		0.0f, 0.0f, 0.0f, 1.0f
	};

	mlVectorMultMatrixTo(a, matrix, result);
}

void
mlVectorRotate(float *a, const float *axis, float angle)
{
	float result[3];

	mlVectorRotateTo(a, axis, angle, result);
	mlVectorCopy(result, a);
}

void
mlVectorAngleTo(const float *a, const float *b, float *angle)
{
	*angle = mlRadToDeg(acosf(mlVectorDotProduct(a, b) / (mlVectorNorm(a) * mlVectorNorm(b))));
}

float
mlVectorAngle(const float *a, const float *b)
{
	return mlRadToDeg(acosf(mlVectorDotProduct(a, b) / (mlVectorNorm(a) * mlVectorNorm(b))));
}
